from template import *

n_time = 20

density_final = 1.0e19

density0 = 0.5 * density_final

time = np.linspace(0, 1.0*n_time, n_time)

density = np.linspace(0, 1.0*n_time, n_time)

p_value_array = np.linspace(0, 1.0*n_time, n_time)

d_value_array = np.linspace(0, 1.0*n_time, n_time)

density_step_array = np.linspace(0, 1.0*n_time, n_time)

density_step = 0.1 * density_final
density_step0 = 0.1 * density_final

density[0] = density0
density[1] = density0 + density_step

kp = 0.5
kd = 0.5

zoom_factor = 1.0

for i in np.arange(2, n_time):
    p_value = -(density[i-1] - density_final) / density_final
    d_value = (density[i-1] - density[i-2]) / density_final
    #zoom_factor = zoom_factor * (kp * p_value + kd * d_value)
    #density_step = density_step * zoom_factor
    density_step = density_step0 * (kp * p_value + kd * d_value)
    print("density_step: ",density_step)
    print("zoom_factor: ",zoom_factor)
    print("p_value: ",p_value)
    print("d_value: ",d_value)
    density[i] = density[i-1] + density_step * 10
    p_value_array[i] = p_value
    d_value_array[i] = d_value
    density_step_array[i] = density_step


##inite the fig of matplotlib
fig=plt.figure(figsize=(10,10))
fig.subplots_adjust(top=0.9,bottom=0.1,wspace=0.5,hspace=0.35)




#density
ax = fig.add_subplot(4,1,1)

line = ax.plot(time[0:n_time], density[0:n_time])

ax.set_ylim(bottom = 0.0)

#p_value
ax = fig.add_subplot(4,1,2)

line = ax.plot(time[0:n_time], p_value_array[0:n_time])


#d_value
ax = fig.add_subplot(4,1,3)

line = ax.plot(time[0:n_time], d_value_array[0:n_time])


#density_step
ax = fig.add_subplot(4,1,4)

line = ax.plot(time[0:n_time], density_step_array[0:n_time])



fig.savefig("pid_control_density.svg")
